#include "camera.hpp"

#include "camera_opencv_receiver.h"
#include "camera_trigger.h"
#include "image_collector.h"

#include "muduo/net/EventLoop.h"
#include "muduo/net/EventLoopThread.h"

using namespace sensor::camera;
using Receiver = sensor::camera::CameraOpencvReceiver;

static muduo::net::EventLoopThread CameraReceiverThread;
static muduo::net::EventLoopThread CameraTriggerThread;

CameraComponent::CameraComponent(const rclcpp::NodeOptions& options)
    : rclcpp::Node("camera", options)
{
    auto logger = rclcpp::get_logger("sensor::camera");
    RCLCPP_INFO(logger, "sensor::camera start!");

    // 以下是相机配置，后续改配置文件
    auto loop = CameraReceiverThread.startLoop();
    video_devs_ = {
        { "camera0", std::make_shared<Receiver>("/dev/video0", loop, 3840, 2160) },
        { "camera1", std::make_shared<Receiver>("/dev/video1", loop, 3840, 2160) },
        { "camera2", std::make_shared<Receiver>("/dev/video2", loop) },
        { "camera3", std::make_shared<Receiver>("/dev/video3", loop) },
        { "camera4", std::make_shared<Receiver>("/dev/video4", loop) },
        { "camera5", std::make_shared<Receiver>("/dev/video5", loop) },
        { "camera6", std::make_shared<Receiver>("/dev/video6", loop) },
    };
   
    // 初始化图片收集器, 对齐多相机数据,保存mp4、推送ros消息
    setupImageCollector();

    // 启动接收器，几个相机就有几个接收器, 这里顺便做了yuv2rgb转换，发给收集器的是cvmat
    setupCameraReceivers();
    // setupCameraTrigger();

    RCLCPP_INFO(logger, "sensor::camera init done!");
}

void CameraComponent::setupImageCollector()
{
    collector_ = std::make_shared<ImageCollector>(video_devs_.size());
}

void CameraComponent::setupCameraReceivers()
{
    for (auto& dev : video_devs_) {
        using namespace std::placeholders;
        auto cb = std::bind(&ImageCollector::infuseOneFrame, collector_.get(), dev.first, _1);
        dev.second->setOneFrameCallback(cb);
    }

    for (auto& dev : video_devs_) {
        dev.second->enable();
    }
}

void CameraComponent::setupCameraTrigger()
{
    trigger_ = std::make_shared<CameraTrigger>(CameraTriggerThread.startLoop(), 0.1);
    trigger_->setTriggerBeginCallback(std::bind(&ImageCollector::reset, collector_.get()));
    trigger_->run();
}

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(sensor::camera::CameraComponent)
